import os
import launch
import launch_ros
import launch.event_handlers
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration


def generate_launch_description():
    use_sim_time = LaunchConfiguration("use_sim_time", default="True")
    map_file = os.path.join(
        get_package_share_directory("nav_pkg"), "maps", "map.yaml"
    )

    # 创建map_server节点
    map_server_cmd = Node(
        package='nav2_map_server',
        executable='map_server',
        name='map_server',
        output='screen',
        parameters=[{'yaml_filename': map_file},
                    {'topic': 'map'},
                    {'frame_id': 'map'},
                    {'output': 'screen'},
                    {'use_sim_time': True}]
    )
    
    
    lifecycle_nodes = ['map_server']
    start_lifecycle_manager_cmd = launch_ros.actions.Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager',
            output='screen',
            emulate_tty=True,  # https://github.com/ros2/launch/issues/188
            parameters=[{'use_sim_time': use_sim_time},
                        {'autostart': True},
                        {'node_names': lifecycle_nodes}])

    # 添加 map -> odom 的静态变换
    static_transform_publisher_cmd = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        arguments=['0','0','0','0','0','0','map','odom'],
        name="static_transform_publisher",
    )
    
    
    rviz_file = os.path.join(
        get_package_share_directory("rviz_tools_pkg"), "rviz", "edit.rviz"
    )
    rviz_cmd = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="screen",
        arguments=["-d", rviz_file],
    )
    # 编辑点位
    edit_point_cmd = Node(
        package='rviz_tools_pkg',
        executable='edit_point',
        name='edit_point',
        output='screen'
    )

    ld = LaunchDescription()
    ld.add_action(map_server_cmd)
    ld.add_action(start_lifecycle_manager_cmd)
    # ld.add_action(static_transform_publisher_cmd) 
    ld.add_action(edit_point_cmd)
    ld.add_action(rviz_cmd)
    
    return ld
